package com.wltt.product.frfront.service;

import com.wltt.product.frfront.config.exception.BusinessException;
import com.wltt.product.frfront.config.exception.RobotException;
import com.wltt.product.frfront.config.robot.WlttRobot;
import com.wltt.product.frfront.constants.WeldTypeConstant;
import com.wltt.product.frfront.enums.PositionTypeEnum;
import com.wltt.product.frfront.executors.*;
import com.wltt.product.frfront.holder.RobotHolder;
import com.wltt.product.frfront.mapper.FormulaMapper;
import com.wltt.product.frfront.mapper.SwingTypeMapper;
import com.wltt.product.frfront.model.ResultData;
import com.wltt.product.frfront.model.bo.FormulaDetailBO;
import com.wltt.product.frfront.model.bo.WeldPointBO;
import com.wltt.product.frfront.model.bo.WeldingJobBO;
import com.wltt.product.frfront.model.po.SwingTypePO;
import com.wltt.product.frfront.model.vo.*;
import com.wltt.product.frfront.model.vo.weld.MarkPosition;
import com.wltt.product.frfront.model.vo.weld.WeldingJob;
import fairino.*;
import jakarta.annotation.Resource;
import org.springframework.http.HttpStatus;
import org.springframework.stereotype.Service;
import org.springframework.util.CollectionUtils;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

@Service
public class WeldServiceImpl implements WeldService {

    @Resource
    private WlttRobot robot;

    @Resource
    private FormulaMapper formulaMapper;

    @Resource
    private SwingTypeMapper swingTypeMapper;

    /**
     * 正向送丝
     *
     * @return
     */
    @Override
    public ResultData forwardWireFeed() {
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        robot.SetForwardWireFeed(0, 1);
        return ResultData.success();
    }

    /**
     * 正向送丝停止
     *
     * @return
     */
    @Override
    public ResultData stopForwardWireFeed() {
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        robot.SetForwardWireFeed(0, 0);
        return ResultData.success();
    }

    /**
     * 反向送丝
     *
     * @return
     */
    @Override
    public ResultData reverseWireFeed() {
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        robot.SetReverseWireFeed(0, 1);
        return ResultData.success();
    }

    /**
     * 反向送丝停止
     *
     * @return
     */
    @Override
    public ResultData stopReverseWireFeed() {
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        robot.SetReverseWireFeed(0, 0);
        return ResultData.success();
    }

    /**
     * 气体检测
     *
     * @return
     */
    @Override
    public ResultData airCheck() {
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        robot.SetAspirated(0, 1);
        robot.Sleep(1000);
        robot.SetAspirated(0, 0);
        return ResultData.success();
    }

    /**
     * 快速直线焊接
     *
     * @param weldREQ
     * @return
     */
    @Override
    public ResultData fastLineWeld(WeldREQ weldREQ) {
        if (RobotHolder.getIsError()) {
            throw new BusinessException("请先清除当前机械臂的异常信息再尝试", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getFormulaId() == null) {
            throw new BusinessException("未选择配方", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingCurrent() == null || weldREQ.getWeldingCurrent() <= 0) {
            throw new BusinessException("焊接电流无效", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingVoltage() == null || weldREQ.getWeldingVoltage() <= 0) {
            throw new BusinessException("焊接电压无效", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingSpeed() == null || weldREQ.getWeldingSpeed() <= 0 || weldREQ.getWeldingSpeed() > 6000) {
            throw new BusinessException("焊接速度无效[1, 6000]", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getSwingTypeId() == null) {
            throw new BusinessException("未选择摆动类型", HttpStatus.BAD_REQUEST);
        }

        List<MarkPosition> markPositions =  weldREQ.getMarkPositions();
        if (CollectionUtils.isEmpty(markPositions)) {
            throw new BusinessException("未标记任何位置信息", HttpStatus.BAD_REQUEST);
        }

        FormulaDetailBO formulaDetailBO = formulaMapper.formulaDetail(weldREQ.getFormulaId());
        if (formulaDetailBO == null) {
            throw new BusinessException("配方不存在", HttpStatus.BAD_REQUEST);
        }

        SwingTypePO swingTypePO = swingTypeMapper.queryById(weldREQ.getSwingTypeId());
        if (swingTypePO == null) {
            throw new BusinessException("摆动类型不存在", HttpStatus.BAD_REQUEST);
        }

        // 排序
        Collections.sort(markPositions, (m1, m2) -> m1.getOrder() - m2.getOrder());

        // 分解点
        WeldPointBO containStartSecurePoint = null;
        WeldPointBO containEndSecurePoint = null;
        WeldPointBO containArcStartPoint = null;
        WeldPointBO containArcEndPoint = null;
        List<WeldPointBO> jointPosList = new ArrayList<>();
        for (MarkPosition markPosition : markPositions) {
            if (PositionTypeEnum.SECURE_START.getCode() == markPosition.getPositionType()) {
                containStartSecurePoint = toWeldPointBO(markPosition);
                continue;
            }
            if (PositionTypeEnum.SECURE_END.getCode() == markPosition.getPositionType()) {
                containEndSecurePoint = toWeldPointBO(markPosition);
                continue;
            }
            if (PositionTypeEnum.ARC_START.getCode() == markPosition.getPositionType()) {
                containArcStartPoint = toWeldPointBO(markPosition);
                continue;
            }
            if (PositionTypeEnum.ARC_END.getCode() == markPosition.getPositionType()) {
                containArcEndPoint = toWeldPointBO(markPosition);
                continue;
            }
            if (PositionTypeEnum.PASS.getCode() == markPosition.getPositionType()) {
                jointPosList.add(toWeldPointBO(markPosition));
            }
        }

        if (null == containStartSecurePoint) {
            throw new BusinessException("开始安全点未标记", HttpStatus.BAD_REQUEST);
        }

        if (null == containArcStartPoint) {
            throw new BusinessException("起弧点未标记", HttpStatus.BAD_REQUEST);
        }

        if (null == containArcEndPoint) {
            throw new BusinessException("收弧点未标记", HttpStatus.BAD_REQUEST);
        }

        if (null == containEndSecurePoint) {
            throw new BusinessException("结束安全点未标记", HttpStatus.BAD_REQUEST);
        }

//        if (CollectionUtils.isEmpty(jointPosList)) {
//            throw new BusinessException("至少标记一个过渡点", HttpStatus.BAD_REQUEST);
//        }

        if (RobotHolder.isBusy()) {
            throw new BusinessException("请等待当前焊接结束", HttpStatus.BAD_REQUEST);
        }

        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }

        FastLineWeldExecutor weldExecutor = new FastLineWeldExecutor(robot, weldREQ,
                containStartSecurePoint, containEndSecurePoint, containArcStartPoint, containArcEndPoint,
                jointPosList, formulaDetailBO, swingTypePO);
        Thread thread = new Thread(weldExecutor);
//        RobotHolder.setRobotJobThread(thread);
        RobotHolder.startWeld();
        thread.start();

        return ResultData.success();
    }

    /**
     * 快速圆弧焊接
     *
     * @param weldREQ
     * @return
     */
    @Override
    public ResultData fastArcWeld(WeldREQ weldREQ) {
        if (RobotHolder.getIsError()) {
            throw new BusinessException("请先清除当前机械臂的异常信息再尝试", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getFormulaId() == null) {
            throw new BusinessException("未选择配方", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingCurrent() == null || weldREQ.getWeldingCurrent() <= 0) {
            throw new BusinessException("焊接电流无效", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingVoltage() == null || weldREQ.getWeldingVoltage() <= 0) {
            throw new BusinessException("焊接电压无效", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingSpeed() == null || weldREQ.getWeldingSpeed() <= 0 || weldREQ.getWeldingSpeed() > 6000) {
            throw new BusinessException("焊接速度无效[1, 6000]", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getSwingTypeId() == null) {
            throw new BusinessException("未选择摆动类型", HttpStatus.BAD_REQUEST);
        }

        List<MarkPosition> markPositions =  weldREQ.getMarkPositions();
        if (CollectionUtils.isEmpty(markPositions)) {
            throw new BusinessException("未标记任何位置信息", HttpStatus.BAD_REQUEST);
        }

        FormulaDetailBO formulaDetailBO = formulaMapper.formulaDetail(weldREQ.getFormulaId());
        if (formulaDetailBO == null) {
            throw new BusinessException("配方不存在", HttpStatus.BAD_REQUEST);
        }

        SwingTypePO swingTypePO = swingTypeMapper.queryById(weldREQ.getSwingTypeId());
        if (swingTypePO == null) {
            throw new BusinessException("摆动类型不存在", HttpStatus.BAD_REQUEST);
        }

        // 排序
        Collections.sort(markPositions, (m1, m2) -> m1.getOrder() - m2.getOrder());

        // 分解点
        WeldPointBO containStartSecurePoint = null;
        WeldPointBO containEndSecurePoint = null;
        WeldPointBO containArcStartPoint = null;
        List<WeldPointBO> jointPosList = new ArrayList<>();
        // 标记上一次点
        int i = 0;
        MarkPosition lastMarkPosition = null;
        for (MarkPosition markPosition : markPositions) {
            if (PositionTypeEnum.SECURE_START.getCode() == markPosition.getPositionType()) {
                containStartSecurePoint = toWeldPointBO(markPosition);
            } else if (PositionTypeEnum.SECURE_END.getCode() == markPosition.getPositionType()) {
                containEndSecurePoint = toWeldPointBO(markPosition);
            } else if (PositionTypeEnum.ARC_START.getCode() == markPosition.getPositionType()) {
                containArcStartPoint = toWeldPointBO(markPosition);
            } else if (PositionTypeEnum.CENTER.getCode() == markPosition.getPositionType()) {
                // 中间点只能跟在起弧点或终点的后面
                if (PositionTypeEnum.ARC_START.getCode() != lastMarkPosition.getPositionType() && PositionTypeEnum.FINISH.getCode() != lastMarkPosition.getPositionType()) {
                    throw new BusinessException("中间点只能跟在起弧点或终点的后面", HttpStatus.BAD_REQUEST);
                }
                jointPosList.add(toWeldPointBO(markPosition));
            } else if (PositionTypeEnum.FINISH.getCode() == markPosition.getPositionType()) {
                // 终点只能在中间点的后面
                if (PositionTypeEnum.CENTER.getCode() != lastMarkPosition.getPositionType()) {
                    throw new BusinessException("终点只能在中间点的后面", HttpStatus.BAD_REQUEST);
                }
                jointPosList.add(toWeldPointBO(markPosition));
            }
            lastMarkPosition = markPosition;
        }

        if (null == containStartSecurePoint) {
            throw new BusinessException("开始安全点未标记", HttpStatus.BAD_REQUEST);
        }

        if (null == containArcStartPoint) {
            throw new BusinessException("起弧点未标记", HttpStatus.BAD_REQUEST);
        }

        if (null == containEndSecurePoint) {
            throw new BusinessException("结束安全点未标记", HttpStatus.BAD_REQUEST);
        }

//        if (jointPosList.size() < 2) {
//            throw new BusinessException("至少标记一个中间点和终点", HttpStatus.BAD_REQUEST);
//        }

        if (jointPosList.size() % 2 != 0) {
            throw new BusinessException("中间点和终点数量必须要一致", HttpStatus.BAD_REQUEST);
        }

        if (RobotHolder.isBusy()) {
            throw new BusinessException("请等待当前焊接结束", HttpStatus.BAD_REQUEST);
        }

        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        FastArcWeldExecutor weldExecutor = new FastArcWeldExecutor(robot, weldREQ,
                containStartSecurePoint, containEndSecurePoint, containArcStartPoint,
                jointPosList, formulaDetailBO, swingTypePO);
        Thread thread = new Thread(weldExecutor);
//        RobotHolder.setRobotJobThread(thread);
        RobotHolder.startWeld();
        thread.start();

        return ResultData.success();
    }

    /**
     * 直线组合焊接
     *
     * @param weldREQ
     * @return
     */
    @Override
    public ResultData lineGroupWeld(WeldREQ weldREQ) {
        if (RobotHolder.getIsError()) {
            throw new BusinessException("请先清除当前机械臂的异常信息再尝试", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getFormulaId() == null) {
            throw new BusinessException("未选择配方", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingCurrent() == null || weldREQ.getWeldingCurrent() <= 0) {
            throw new BusinessException("焊接电流无效", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingVoltage() == null || weldREQ.getWeldingVoltage() <= 0) {
            throw new BusinessException("焊接电压无效", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingSpeed() == null || weldREQ.getWeldingSpeed() <= 0 || weldREQ.getWeldingSpeed() > 6000) {
            throw new BusinessException("焊接速度无效[1, 6000]", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getSwingTypeId() == null) {
            throw new BusinessException("未选择摆动类型", HttpStatus.BAD_REQUEST);
        }

        List<MarkPosition> markPositions =  weldREQ.getMarkPositions();
        if (CollectionUtils.isEmpty(markPositions)) {
            throw new BusinessException("未标记任何位置信息", HttpStatus.BAD_REQUEST);
        }

        FormulaDetailBO formulaDetailBO = formulaMapper.formulaDetail(weldREQ.getFormulaId());
        if (formulaDetailBO == null) {
            throw new BusinessException("配方不存在", HttpStatus.BAD_REQUEST);
        }

        SwingTypePO swingTypePO = swingTypeMapper.queryById(weldREQ.getSwingTypeId());
        if (swingTypePO == null) {
            throw new BusinessException("摆动类型不存在", HttpStatus.BAD_REQUEST);
        }

        // 排序
        Collections.sort(markPositions, (m1, m2) -> m1.getOrder() - m2.getOrder());

        // 分解点
        WeldPointBO containStartSecurePoint = null;
        WeldPointBO containEndSecurePoint = null;
        WeldPointBO containArcStartPoint = null;
        WeldPointBO containArcEndPoint = null;
        List<WeldPointBO> jointPosList = new ArrayList<>();
        for (MarkPosition markPosition : markPositions) {
            if (PositionTypeEnum.SECURE_START.getCode() == markPosition.getPositionType()) {
                containStartSecurePoint = toWeldPointBO(markPosition);
                continue;
            }
            if (PositionTypeEnum.SECURE_END.getCode() == markPosition.getPositionType()) {
                containEndSecurePoint = toWeldPointBO(markPosition);
                continue;
            }
            if (PositionTypeEnum.ARC_START.getCode() == markPosition.getPositionType()) {
                containArcStartPoint = toWeldPointBO(markPosition);
                continue;
            }
            if (PositionTypeEnum.ARC_END.getCode() == markPosition.getPositionType()) {
                containArcEndPoint = toWeldPointBO(markPosition);
                continue;
            }
            if (PositionTypeEnum.PASS.getCode() == markPosition.getPositionType()) {
                jointPosList.add(toWeldPointBO(markPosition));
            }
        }

        if (null == containStartSecurePoint) {
            throw new BusinessException("开始安全点未标记", HttpStatus.BAD_REQUEST);
        }

        if (null == containArcStartPoint) {
            throw new BusinessException("起弧点未标记", HttpStatus.BAD_REQUEST);
        }

        if (null == containArcEndPoint) {
            throw new BusinessException("收弧点未标记", HttpStatus.BAD_REQUEST);
        }

        if (null == containEndSecurePoint) {
            throw new BusinessException("结束安全点未标记", HttpStatus.BAD_REQUEST);
        }

//        if (CollectionUtils.isEmpty(jointPosList)) {
//            throw new BusinessException("至少标记一个过渡点", HttpStatus.BAD_REQUEST);
//        }

        if (RobotHolder.isBusy()) {
            throw new BusinessException("请等待当前焊接结束", HttpStatus.BAD_REQUEST);
        }
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        FastLineWeldExecutor weldExecutor = new FastLineWeldExecutor(robot, weldREQ,
                containStartSecurePoint, containEndSecurePoint, containArcStartPoint, containArcEndPoint,
                jointPosList, formulaDetailBO, swingTypePO);
        Thread thread = new Thread(weldExecutor);
//        RobotHolder.setRobotJobThread(thread);
        RobotHolder.startWeld();
        thread.start();

        return ResultData.success();
    }

    /**
     * 直线圆弧组合焊接
     *
     * @param weldREQ
     * @return
     */
    @Override
    public ResultData lineAndArcGroupWeld(WeldREQ weldREQ) {
        if (RobotHolder.getIsError()) {
            throw new BusinessException("请先清除当前机械臂的异常信息再尝试", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getFormulaId() == null) {
            throw new BusinessException("未选择配方", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingCurrent() == null || weldREQ.getWeldingCurrent() <= 0) {
            throw new BusinessException("焊接电流无效", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingVoltage() == null || weldREQ.getWeldingVoltage() <= 0) {
            throw new BusinessException("焊接电压无效", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingSpeed() == null || weldREQ.getWeldingSpeed() <= 0 || weldREQ.getWeldingSpeed() > 6000) {
            throw new BusinessException("焊接速度无效[1, 6000]", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getSwingTypeId() == null) {
            throw new BusinessException("未选择摆动类型", HttpStatus.BAD_REQUEST);
        }

        List<MarkPosition> markPositions =  weldREQ.getMarkPositions();
        if (CollectionUtils.isEmpty(markPositions)) {
            throw new BusinessException("未标记任何位置信息", HttpStatus.BAD_REQUEST);
        }

        FormulaDetailBO formulaDetailBO = formulaMapper.formulaDetail(weldREQ.getFormulaId());
        if (formulaDetailBO == null) {
            throw new BusinessException("配方不存在", HttpStatus.BAD_REQUEST);
        }

        SwingTypePO swingTypePO = swingTypeMapper.queryById(weldREQ.getSwingTypeId());
        if (swingTypePO == null) {
            throw new BusinessException("摆动类型不存在", HttpStatus.BAD_REQUEST);
        }

        // 排序
        Collections.sort(markPositions, (m1, m2) -> m1.getOrder() - m2.getOrder());

        // 分解点
        WeldPointBO containStartSecurePoint = null;
        WeldPointBO containEndSecurePoint = null;
        WeldPointBO containArcStartPoint = null;
        WeldPointBO containArcEndPoint = null;
        List<WeldPointBO> jointPosList = new ArrayList<>();
        int centerCount = 0;
        int finishCount = 0;
        MarkPosition nextMarkPosition = null;
        for (int i = 0; i < markPositions.size(); i++) {
            MarkPosition markPosition = markPositions.get(i);
            if (i + 1 < markPositions.size()) {
                nextMarkPosition = markPositions.get(i + 1) ;
            } else {
                nextMarkPosition = null;
            }
            if (PositionTypeEnum.SECURE_START.getCode() == markPosition.getPositionType()) {
                containStartSecurePoint = toWeldPointBO(markPosition);
            } else if (PositionTypeEnum.SECURE_END.getCode() == markPosition.getPositionType()) {
                containEndSecurePoint = toWeldPointBO(markPosition);
            } else if (PositionTypeEnum.ARC_START.getCode() == markPosition.getPositionType()) {
                containArcStartPoint = toWeldPointBO(markPosition);
            } else if (PositionTypeEnum.ARC_END.getCode() == markPosition.getPositionType()) {
                containArcEndPoint = toWeldPointBO(markPosition);
            } else if (PositionTypeEnum.PASS.getCode() == markPosition.getPositionType()) {
                jointPosList.add(toWeldPointBO(markPosition));
            } else if (PositionTypeEnum.CENTER.getCode() == markPosition.getPositionType()) {
                // 中间点后面必须紧跟着终点
                if (PositionTypeEnum.FINISH.getCode() != nextMarkPosition.getPositionType()) {
                    throw new BusinessException("中间点后面必须紧跟着终点", HttpStatus.BAD_REQUEST);
                }
                jointPosList.add(toWeldPointBO(markPosition));
                centerCount = centerCount + 1;
            } else if (PositionTypeEnum.FINISH.getCode() == markPosition.getPositionType()) {
                jointPosList.add(toWeldPointBO(markPosition));
                finishCount = finishCount + 1;
            }
        }

        if (finishCount < centerCount) {
            throw new BusinessException("请检查中间点和终点关系", HttpStatus.BAD_REQUEST);
        }

        if (null == containStartSecurePoint) {
            throw new BusinessException("开始安全点未标记", HttpStatus.BAD_REQUEST);
        }

        if (null == containArcStartPoint) {
            throw new BusinessException("起弧点未标记", HttpStatus.BAD_REQUEST);
        }

        if (null == containArcEndPoint) {
            throw new BusinessException("收弧点未标记", HttpStatus.BAD_REQUEST);
        }

        if (null == containEndSecurePoint) {
            throw new BusinessException("结束安全点未标记", HttpStatus.BAD_REQUEST);
        }

        if (RobotHolder.isBusy()) {
            throw new BusinessException("请等待当前焊接结束", HttpStatus.BAD_REQUEST);
        }
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        LineAndArcWeldExecutor weldExecutor = new LineAndArcWeldExecutor(robot, weldREQ,
                containStartSecurePoint, containEndSecurePoint, containArcStartPoint, containArcEndPoint,
                jointPosList, formulaDetailBO, swingTypePO);
        Thread thread = new Thread(weldExecutor);
//        RobotHolder.setRobotJobThread(thread);
        RobotHolder.startWeld();
        thread.start();

        return ResultData.success();
    }

    /**
     * 分段组合焊接
     *
     * @param weldREQ
     * @return
     */
    @Override
    public ResultData paragraphGroupWeld(WeldREQ weldREQ) {
        if (RobotHolder.getIsError()) {
            throw new BusinessException("请先清除当前机械臂的异常信息再尝试", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getFormulaId() == null) {
            throw new BusinessException("未选择配方", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingCurrent() == null || weldREQ.getWeldingCurrent() <= 0) {
            throw new BusinessException("焊接电流无效", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingVoltage() == null || weldREQ.getWeldingVoltage() <= 0) {
            throw new BusinessException("焊接电压无效", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getWeldingSpeed() == null || weldREQ.getWeldingSpeed() <= 0 || weldREQ.getWeldingSpeed() > 6000) {
            throw new BusinessException("焊接速度无效[1, 6000]", HttpStatus.BAD_REQUEST);
        }
        if (weldREQ.getSwingTypeId() == null) {
            throw new BusinessException("未选择摆动类型", HttpStatus.BAD_REQUEST);
        }

        List<MarkPosition> markPositions =  weldREQ.getMarkPositions();
        if (CollectionUtils.isEmpty(markPositions)) {
            throw new BusinessException("未标记任何位置信息", HttpStatus.BAD_REQUEST);
        }

        FormulaDetailBO formulaDetailBO = formulaMapper.formulaDetail(weldREQ.getFormulaId());
        if (formulaDetailBO == null) {
            throw new BusinessException("配方不存在", HttpStatus.BAD_REQUEST);
        }

        SwingTypePO swingTypePO = swingTypeMapper.queryById(weldREQ.getSwingTypeId());
        if (swingTypePO == null) {
            throw new BusinessException("摆动类型不存在", HttpStatus.BAD_REQUEST);
        }

        // 排序
        Collections.sort(markPositions, (m1, m2) -> m1.getOrder() - m2.getOrder());

        // 分解点
        WeldPointBO containStartSecurePoint = null;
        WeldPointBO containEndSecurePoint = null;
        List<WeldPointBO> jointPosList = new ArrayList<>();
        int arcStartCount = 0;
        int arcEndCount = 0;
        for (MarkPosition markPosition : markPositions) {
            if (PositionTypeEnum.SECURE_START.getCode() == markPosition.getPositionType()) {
                containStartSecurePoint = toWeldPointBO(markPosition);
                continue;
            }
            if (PositionTypeEnum.SECURE_END.getCode() == markPosition.getPositionType()) {
                containEndSecurePoint = toWeldPointBO(markPosition);
                continue;
            }
            if (PositionTypeEnum.ARC_START.getCode() == markPosition.getPositionType()) {
                jointPosList.add(toWeldPointBO(markPosition));
                arcStartCount++;
            }
            if (PositionTypeEnum.ARC_END.getCode() == markPosition.getPositionType()) {
                jointPosList.add(toWeldPointBO(markPosition));
                arcEndCount++;
            }
        }

        if (null == containStartSecurePoint) {
            throw new BusinessException("开始安全点未标记", HttpStatus.BAD_REQUEST);
        }

        if (null == containEndSecurePoint) {
            throw new BusinessException("结束安全点未标记", HttpStatus.BAD_REQUEST);
        }

        if (CollectionUtils.isEmpty(jointPosList)) {
            throw new BusinessException("至少标记一个起弧点和收弧点", HttpStatus.BAD_REQUEST);
        }

        if (arcStartCount != arcEndCount) {
            throw new BusinessException("起弧点和收弧点数量不一致", HttpStatus.BAD_REQUEST);
        }

        if (RobotHolder.isBusy()) {
            throw new BusinessException("请等待当前焊接结束", HttpStatus.BAD_REQUEST);
        }
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        ParagraphGroupWeldExecutor weldExecutor = new ParagraphGroupWeldExecutor(robot, weldREQ,
                containStartSecurePoint, containEndSecurePoint,
                jointPosList, formulaDetailBO, swingTypePO);
        Thread thread = new Thread(weldExecutor);
//        RobotHolder.setRobotJobThread(thread);
        RobotHolder.startWeld();
        thread.start();

        return ResultData.success();
    }

    /**
     * 通用组合焊接
     *
     * @param commonWeldREQ
     * @return
     */
    @Override
    public ResultData commonGroupWeld(CommonWeldREQ commonWeldREQ) {
        if (RobotHolder.getIsError()) {
            throw new BusinessException("请先清除当前机械臂的异常信息再尝试", HttpStatus.BAD_REQUEST);
        }
        if (commonWeldREQ.getFormulaId() == null) {
            throw new BusinessException("未选择配方", HttpStatus.BAD_REQUEST);
        }
        if (commonWeldREQ.getWeldingCurrent() == null || commonWeldREQ.getWeldingCurrent() <= 0) {
            throw new BusinessException("焊接电流无效", HttpStatus.BAD_REQUEST);
        }
        if (commonWeldREQ.getWeldingVoltage() == null || commonWeldREQ.getWeldingVoltage() <= 0) {
            throw new BusinessException("焊接电压无效", HttpStatus.BAD_REQUEST);
        }
        if (commonWeldREQ.getWeldingSpeed() == null || commonWeldREQ.getWeldingSpeed() <= 0 || commonWeldREQ.getWeldingSpeed() > 6000) {
            throw new BusinessException("焊接速度无效[1, 6000]", HttpStatus.BAD_REQUEST);
        }
        if (commonWeldREQ.getSwingTypeId() == null) {
            throw new BusinessException("未选择摆动类型", HttpStatus.BAD_REQUEST);
        }

        List<WeldingJob> weldingJobs =  commonWeldREQ.getWeldingJobs();
        if (CollectionUtils.isEmpty(weldingJobs)) {
            throw new BusinessException("无焊接任务", HttpStatus.BAD_REQUEST);
        }

        FormulaDetailBO formulaDetailBO = formulaMapper.formulaDetail(commonWeldREQ.getFormulaId());
        if (formulaDetailBO == null) {
            throw new BusinessException("配方不存在", HttpStatus.BAD_REQUEST);
        }

        SwingTypePO swingTypePO = swingTypeMapper.queryById(commonWeldREQ.getSwingTypeId());
        if (swingTypePO == null) {
            throw new BusinessException("摆动类型不存在", HttpStatus.BAD_REQUEST);
        }

        // 排序
        Collections.sort(weldingJobs, (m1, m2) -> m1.getJobOrder() - m2.getJobOrder());

        List<WeldingJobBO> weldingJobList = new ArrayList<>();
        WeldingJob last = null;
        for (WeldingJob weldingJob : weldingJobs) {
            WeldingJobBO weldingJobBO = new WeldingJobBO();
            weldingJobBO.setJobOrder(weldingJob.getJobOrder());
            weldingJobBO.setWeldType(weldingJob.getWeldType());
            weldingJobBO.setIsContinue(weldingJob.getIsContinue());
            List<WeldPointBO> jointPosList = new ArrayList<>();
            List<MarkPosition> markPositions = weldingJob.getMarkPositions();
            // 排序
            Collections.sort(markPositions, (m1, m2) -> m1.getOrder() - m2.getOrder());
            MarkPosition lastMarkPosition = null;
            int passCount = 0;
            int centerCount = 0;
            int finishCount = 0;
            for (MarkPosition markPosition : markPositions) {
                if (PositionTypeEnum.SECURE_START.getCode() == markPosition.getPositionType()) {
                    weldingJobBO.setStartSecurePoint(toWeldPointBO(markPosition));
                } else if (PositionTypeEnum.SECURE_END.getCode() == markPosition.getPositionType()) {
                    weldingJobBO.setEndSecurePoint(toWeldPointBO(markPosition));
                } else if (PositionTypeEnum.ARC_START.getCode() == markPosition.getPositionType()) {
                    weldingJobBO.setArcStartPoint(toWeldPointBO(markPosition));
                } else if (PositionTypeEnum.ARC_END.getCode() == markPosition.getPositionType()) {
                    weldingJobBO.setArcEndPoint(toWeldPointBO(markPosition));
                } else if (WeldTypeConstant.Line == weldingJobBO.getWeldType() && PositionTypeEnum.PASS.getCode() == markPosition.getPositionType()) {
                    jointPosList.add(toWeldPointBO(markPosition));
                    passCount++;
                } else if (WeldTypeConstant.Circle == weldingJobBO.getWeldType()) {
                    if (PositionTypeEnum.CENTER.getCode() == markPosition.getPositionType()) {
                        if (lastMarkPosition != null && (PositionTypeEnum.ARC_START.getCode() == lastMarkPosition.getPositionType() ||  PositionTypeEnum.FINISH.getCode() == lastMarkPosition.getPositionType())) {
                            jointPosList.add(toWeldPointBO(markPosition));
                            centerCount++;
                        } else {
                            throw new BusinessException("中间点只能在起弧点或者终点后面", HttpStatus.BAD_REQUEST);
                        }
                    } else if (PositionTypeEnum.FINISH.getCode() == markPosition.getPositionType()) {
                        if (lastMarkPosition != null && PositionTypeEnum.CENTER.getCode() == lastMarkPosition.getPositionType()) {
                            jointPosList.add(toWeldPointBO(markPosition));
                            finishCount++;
                        } else {
                            throw new BusinessException("终点只能在中间点后面", HttpStatus.BAD_REQUEST);
                        }
                    }
                }
                lastMarkPosition = markPosition;
            }
            weldingJobBO.setJointPosList(jointPosList);

            if ((null == last || !last.getIsContinue()) && null == weldingJobBO.getStartSecurePoint()) {
                throw new BusinessException("上一个非连续焊接，开始安全点必须标记", HttpStatus.BAD_REQUEST);
            }

            if (null != last && last.getIsContinue() && null != weldingJobBO.getStartSecurePoint()) {
                throw new BusinessException("上一个为连续焊接，安全点无须标记", HttpStatus.BAD_REQUEST);
            }

            if (null == weldingJobBO.getArcStartPoint()) {
                throw new BusinessException("起弧点未标记", HttpStatus.BAD_REQUEST);
            }

            if (!weldingJobBO.getIsContinue() && null == weldingJobBO.getEndSecurePoint()) {
                throw new BusinessException("非连续焊接点，结束安全点必须标记", HttpStatus.BAD_REQUEST);
            }

            if (weldingJobBO.getIsContinue() && null != weldingJobBO.getEndSecurePoint()) {
                throw new BusinessException("连续焊接点，结束安全点无须标记", HttpStatus.BAD_REQUEST);
            }

            if (WeldTypeConstant.Line == weldingJobBO.getWeldType()) {
//                if (passCount < 1) {
//                    throw new BusinessException("直线焊接至少一个过渡点", HttpStatus.BAD_REQUEST);
//                }
                if (null == weldingJobBO.getArcEndPoint()) {
                    throw new BusinessException("收弧点未标记", HttpStatus.BAD_REQUEST);
                }
            }

            if (WeldTypeConstant.Circle == weldingJobBO.getWeldType()) {
                if (centerCount < 1 || finishCount < 1) {
                    throw new BusinessException("圆弧焊接至少一个中间点和终点", HttpStatus.BAD_REQUEST);
                }
                if (centerCount != finishCount) {
                    throw new BusinessException("圆弧焊接中间点和终点需要一一对应", HttpStatus.BAD_REQUEST);
                }
            }
            weldingJobList.add(weldingJobBO);
            last = weldingJob;
        }

        if (RobotHolder.isBusy()) {
            throw new BusinessException("请等待当前焊接结束", HttpStatus.BAD_REQUEST);
        }
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        CommonGroupWeldExecutor fastLineWeldExecutor = new CommonGroupWeldExecutor(robot, commonWeldREQ,
                formulaDetailBO, swingTypePO, weldingJobList);
        Thread thread = new Thread(fastLineWeldExecutor);
//        RobotHolder.setRobotJobThread(thread);
        RobotHolder.startWeld();
        thread.start();
        return ResultData.success();
    }

    /**
     * 通用组合焊接点位校验
     *
     * @param commonWeldPostCheckREQ
     * @return
     */
    @Override
    public ResultData commonGroupWeldPostCheck(CommonWeldPostCheckREQ commonWeldPostCheckREQ) {
        List<WeldingJob> weldingJobs =  commonWeldPostCheckREQ.getWeldingJobs();
        if (CollectionUtils.isEmpty(weldingJobs)) {
            throw new BusinessException("无焊接任务", HttpStatus.BAD_REQUEST);
        }

        if (commonWeldPostCheckREQ.getCurrentJobOrder() == null) {
            throw new BusinessException("未指定需要校验的任务", HttpStatus.BAD_REQUEST);
        }

        // 排序
        Collections.sort(weldingJobs, (m1, m2) -> m1.getJobOrder() - m2.getJobOrder());
        WeldingJob last = null;
        for (WeldingJob weldingJob : weldingJobs) {
            if (weldingJob.getJobOrder() != commonWeldPostCheckREQ.getCurrentJobOrder()) {
                last = weldingJob;
                continue;
            }
            WeldingJobBO weldingJobBO = new WeldingJobBO();
            weldingJobBO.setJobOrder(weldingJob.getJobOrder());
            weldingJobBO.setWeldType(weldingJob.getWeldType());
            weldingJobBO.setIsContinue(weldingJob.getIsContinue());
            List<MarkPosition> markPositions = weldingJob.getMarkPositions();
            // 排序
            Collections.sort(markPositions, (m1, m2) -> m1.getOrder() - m2.getOrder());
            MarkPosition lastMarkPosition = null;
            int passCount = 0;
            int centerCount = 0;
            int finishCount = 0;
            for (MarkPosition markPosition : markPositions) {
                if (PositionTypeEnum.SECURE_START.getCode() == markPosition.getPositionType()) {
                    weldingJobBO.setStartSecurePoint(toWeldPointBO(markPosition));
                } else if (PositionTypeEnum.SECURE_END.getCode() == markPosition.getPositionType()) {
                    weldingJobBO.setEndSecurePoint(toWeldPointBO(markPosition));
                } else if (PositionTypeEnum.ARC_START.getCode() == markPosition.getPositionType()) {
                    weldingJobBO.setArcStartPoint(toWeldPointBO(markPosition));
                } else if (PositionTypeEnum.ARC_END.getCode() == markPosition.getPositionType()) {
                    weldingJobBO.setArcEndPoint(toWeldPointBO(markPosition));
                } else if (WeldTypeConstant.Line == weldingJobBO.getWeldType() && PositionTypeEnum.PASS.getCode() == markPosition.getPositionType()) {
                    passCount++;
                } else if (WeldTypeConstant.Circle == weldingJobBO.getWeldType()) {
                    if (PositionTypeEnum.CENTER.getCode() == markPosition.getPositionType()) {
                        if (lastMarkPosition != null && (PositionTypeEnum.ARC_START.getCode() == lastMarkPosition.getPositionType() ||  PositionTypeEnum.FINISH.getCode() == lastMarkPosition.getPositionType())) {
                            centerCount++;
                        } else {
                            throw new BusinessException("中间点只能在起弧点或者终点后面", HttpStatus.BAD_REQUEST);
                        }
                    } else if (PositionTypeEnum.FINISH.getCode() == markPosition.getPositionType()) {
                        if (lastMarkPosition != null && PositionTypeEnum.CENTER.getCode() == lastMarkPosition.getPositionType()) {
                            finishCount++;
                        } else {
                            throw new BusinessException("终点只能在中间点后面", HttpStatus.BAD_REQUEST);
                        }
                    }
                }
                lastMarkPosition = markPosition;
            }

            if ((null == last || !last.getIsContinue()) && null == weldingJobBO.getStartSecurePoint()) {
                throw new BusinessException("上一个非连续焊接，开始安全点必须标记", HttpStatus.BAD_REQUEST);
            }

            if (null != last && last.getIsContinue() && null != weldingJobBO.getStartSecurePoint()) {
                throw new BusinessException("上一个为连续焊接，安全点无须标记", HttpStatus.BAD_REQUEST);
            }

            if (null == weldingJobBO.getArcStartPoint()) {
                throw new BusinessException("起弧点未标记", HttpStatus.BAD_REQUEST);
            }

            if (!weldingJobBO.getIsContinue() && null == weldingJobBO.getEndSecurePoint()) {
                throw new BusinessException("非连续焊接点，结束安全点必须标记", HttpStatus.BAD_REQUEST);
            }

            if (weldingJobBO.getIsContinue() && null != weldingJobBO.getEndSecurePoint()) {
                throw new BusinessException("连续焊接点，结束安全点无须标记", HttpStatus.BAD_REQUEST);
            }

            if (WeldTypeConstant.Line == weldingJobBO.getWeldType()) {
                if (null == weldingJobBO.getArcEndPoint()) {
                    throw new BusinessException("收弧点未标记", HttpStatus.BAD_REQUEST);
                }
            }

            if (WeldTypeConstant.Circle == weldingJobBO.getWeldType()) {
                if (centerCount < 1 || finishCount < 1) {
                    throw new BusinessException("圆弧焊接至少一个中间点和终点", HttpStatus.BAD_REQUEST);
                }
                if (centerCount != finishCount) {
                    throw new BusinessException("圆弧焊接中间点和终点需要一一对应", HttpStatus.BAD_REQUEST);
                }
            }
            break;
        }
        return ResultData.success();
    }

    /**
     * 停止焊接接口
     *
     * @return
     */
    @Override
    public ResultData stopWeld() {
        if (robot.isConnected()) {
            throw new BusinessException("机器人未连接或已断开", HttpStatus.BAD_REQUEST);
        }
        if (RobotHolder.getIsError()) {
            throw new BusinessException("请先清除当前机械臂的异常信息再尝试", HttpStatus.BAD_REQUEST);
        }
        RobotHolder.stopWeld();
        try {
            robot.StopMotion();
        } catch (RobotException e) {
            throw new BusinessException(e.getMessage(), HttpStatus.BAD_REQUEST);
        }
        return ResultData.success();
    }


    /**
     * 将页面传过来的点转化成机器人需要的点
     *
     * @param markPosition
     * @return
     */
    private WeldPointBO toWeldPointBO(MarkPosition markPosition) {
        if (markPosition == null) {
            return null;
        }
        WeldPointBO weldPointBO = new WeldPointBO();
        weldPointBO.setPositionType(markPosition.getPositionType());


        Rpy rpy = new Rpy(new BigDecimal(markPosition.getDescTran().getRx()).setScale(3, RoundingMode.HALF_UP).doubleValue(),
                new BigDecimal(markPosition.getDescTran().getRy()).setScale(3, RoundingMode.HALF_UP).doubleValue(),
                new BigDecimal(markPosition.getDescTran().getRz()).setScale(3, RoundingMode.HALF_UP).doubleValue());
        DescTran tran =  new DescTran(new BigDecimal(markPosition.getDescTran().getX()).setScale(3, RoundingMode.HALF_UP).doubleValue(),
                new BigDecimal(markPosition.getDescTran().getY()).setScale(3, RoundingMode.HALF_UP).doubleValue(),
                new BigDecimal(markPosition.getDescTran().getZ()).setScale(3, RoundingMode.HALF_UP).doubleValue());
        DescPose descPose = new DescPose(tran, rpy);
        weldPointBO.setDescPose(descPose);

        JointPos jointPos = new JointPos();
//        jointPos.setJ1(markPosition.getJointPos().getJ1());
//        jointPos.setJ2(markPosition.getJointPos().getJ2());
//        jointPos.setJ3(markPosition.getJointPos().getJ3());
//        jointPos.setJ4(markPosition.getJointPos().getJ4());
//        jointPos.setJ5(markPosition.getJointPos().getJ5());
//        jointPos.setJ6(markPosition.getJointPos().getJ6());


//        robot.GetForwardKin(jointPos,descPose);
        robot.GetInverseKin(0, descPose, -1, jointPos);
        weldPointBO.setJointPos(jointPos);

        return weldPointBO;
    }
}
